package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.debugger.Log;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;

public class Drive {

    public static CANJaguar createJag(int slot){
        try{
            return new CANJaguar(slot);
        }catch(Exception e){
            System.out.println("CANJag "+slot+" shat the bed while initializing");
            Log.log("CANJag "+slot+" shat the bed while initializing");
            return null;
        }
    }
    public static void setJag(CANJaguar jag,double set){
        try{
            jag.setX(set);
        }catch(Exception e){
            System.out.println("CANJag "+jag+" shat the bed while setting");
            Log.log("CANJag "+jag+" shat the bed while setting");
        }
    }
    public static double readJag(CANJaguar jag){
        try{
            return jag.getX();
        }catch(Exception e){
            System.out.println("CANJag "+jag+" shat the bed while reading");
            Log.log("CANJag "+jag+" shat the bed while reading");
            return 0;
        }
    }
    public static void manualDrive(Joystick joy,CANJaguar fl,CANJaguar fr,CANJaguar bl,CANJaguar br,double power,boolean inverse){
        try{
            setJag(fl,-joy.getRawAxis(inverse?RobotMap.DUALACTION_RIGHT_Y:RobotMap.DUALACTION_LEFT_Y)*power*(inverse?-1:1));
            setJag(fr,joy.getRawAxis(inverse?RobotMap.DUALACTION_LEFT_Y:RobotMap.DUALACTION_RIGHT_Y)*power*(inverse?-1:1));
            setJag(bl,-joy.getRawAxis(inverse?RobotMap.DUALACTION_RIGHT_Y:RobotMap.DUALACTION_LEFT_Y)*power*(inverse?-1:1));
            setJag(br,joy.getRawAxis(inverse?RobotMap.DUALACTION_LEFT_Y:RobotMap.DUALACTION_RIGHT_Y)*power*(inverse?-1:1));
        }catch(Exception e){
            System.out.println("Drive CANJags shat the bed while driving");
            Log.log("Drive CANJags shat the bed while driving");
        }
    }
    public static void manualDrop(Joystick joy,Servo s,double lastSet){
        /*if(s.get()==lastSet){
            s.set(!joy.getRawButton(RobotMap.DUALACTION_BUTTON_A)?!joy.getRawButton(RobotMap.DUALACTION_BUTTON_B)?RobotMap.V_SERVO_DROPPER_DEF:RobotMap.V_SERVO_DROPPER_DOWN:RobotMap.V_SERVO_DROPPER_UP);
            return s.get();
        }else{
            return lastSet;
        }*/
        s.set(!joy.getRawButton(RobotMap.DUALACTION_BUTTON_A)?1:0.02);

    }
    public static boolean toggleTest(Joystick j,int p,int b,boolean[][][] bool){
        if(bool[p-1][b-1][0]){
            if(!j.getRawButton(b)){
                bool[p-1][b-1][0]=false;
                bool[p-1][b-1][1]=!bool[p-1][b-1][1];
            }
        }else{
            if(j.getRawButton(b))bool[p-1][b-1][0]=true;
        }
        return bool[p-1][b-1][1];
    }
}